/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                        Intel License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000, Intel Corporation, all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/

#include "precomp.hpp"

#include <float.h>
#include <limits.h>

/* Valery Mosyagin */

typedef void (*pointer_LMJac)( const CvMat* src, CvMat* dst );
typedef void (*pointer_LMFunc)( const CvMat* src, CvMat* dst );

void cvLevenbergMarquardtOptimization(pointer_LMJac JacobianFunction,
									  pointer_LMFunc function,
									  /*pointer_Err error_function,*/
									  CvMat* X0, CvMat* observRes, CvMat* resultX,
									  int maxIter, double epsilon);

void icvReconstructPointsFor3View( CvMat* projMatr1, CvMat* projMatr2, CvMat* projMatr3,
								   CvMat* projPoints1, CvMat* projPoints2, CvMat* projPoints3,
								   CvMat* points4D);


/* Jacobian computation for trifocal case */
void icvJacobianFunction_ProjTrifocal(const CvMat* vectX, CvMat* Jacobian) {
	CV_FUNCNAME( "icvJacobianFunction_ProjTrifocal" );
	__BEGIN__;

	/* Test data for errors */
	if ( vectX == 0 || Jacobian == 0 ) {
		CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
	}

	if ( !CV_IS_MAT(vectX) || !CV_IS_MAT(Jacobian) ) {
		CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
	}

	int numPoints;
	numPoints = (vectX->rows - 36) / 4;

	if ( numPoints < 1 ) { //!!! Need to correct this minimal number of points
		CV_ERROR( CV_StsUnmatchedSizes, "number of points must be more than 0" );
	}

	if ( Jacobian->rows == numPoints * 6 || Jacobian->cols != 36 + numPoints * 4 ) {
		CV_ERROR( CV_StsUnmatchedSizes, "Size of Jacobian is not correct it must be 6*numPoints x (36+numPoints*4)" );
	}

	/* Computed Jacobian in a given point */
	/* This is for function with 3 projection matrices */
	/* vector X consists of projection matrices and points3D */
	/* each 3D points has X,Y,Z,W */
	/* each projection matrices has 3x4 coeffs */
	/* For N points 4D we have Jacobian 2N x (12*3+4N) */

	/* Will store derivates as  */
	/* Fill Jacobian matrix */
	int currProjPoint;
	int currMatr;

	cvZero(Jacobian);
	for ( currMatr = 0; currMatr < 3; currMatr++ ) {
		double p[12];
		for ( int i = 0; i < 12; i++ ) {
			p[i] = cvmGet(vectX, currMatr * 12 + i, 0);
		}

		int currVal = 36;
		for ( currProjPoint = 0; currProjPoint < numPoints; currProjPoint++ ) {
			/* Compute */
			double X[4];
			X[0] = cvmGet(vectX, currVal++, 0);
			X[1] = cvmGet(vectX, currVal++, 0);
			X[2] = cvmGet(vectX, currVal++, 0);
			X[3] = cvmGet(vectX, currVal++, 0);

			double piX[3];
			piX[0] = X[0] * p[0] + X[1] * p[1] + X[2] * p[2]  + X[3] * p[3];
			piX[1] = X[0] * p[4] + X[1] * p[5] + X[2] * p[6]  + X[3] * p[7];
			piX[2] = X[0] * p[8] + X[1] * p[9] + X[2] * p[10] + X[3] * p[11];

			int i, j;
			/* fill derivate by point */

			double tmp3 = 1 / (piX[2] * piX[2]);

			double tmp1 = -piX[0] * tmp3;
			double tmp2 = -piX[1] * tmp3;
			for ( j = 0; j < 2; j++ ) { //for x and y
				for ( i = 0; i < 4; i++ ) { // for X,Y,Z,W
					cvmSet( Jacobian,
							currMatr * numPoints * 2 + currProjPoint * 2 + j, 36 + currProjPoint * 4 + i,
							(p[j*4+i]*piX[2] - p[8+i]*piX[j]) * tmp3  );
				}
			}
			/* fill derivate by projection matrix */
			for ( i = 0; i < 4; i++ ) {
				/* derivate for x */
				cvmSet(Jacobian, currMatr * numPoints * 2 + currProjPoint * 2, currMatr * 12 + i, X[i] / piX[2]); //x' p1i
				cvmSet(Jacobian, currMatr * numPoints * 2 + currProjPoint * 2, currMatr * 12 + 8 + i, X[i]*tmp1); //x' p3i

				/* derivate for y */
				cvmSet(Jacobian, currMatr * numPoints * 2 + currProjPoint * 2 + 1, currMatr * 12 + 4 + i, X[i] / piX[2]); //y' p2i
				cvmSet(Jacobian, currMatr * numPoints * 2 + currProjPoint * 2 + 1, currMatr * 12 + 8 + i, X[i]*tmp2); //y' p3i
			}

		}
	}

	__END__;
	return;
}

void icvFunc_ProjTrifocal(const CvMat* vectX, CvMat* resFunc) {
	/* Computes function in a given point */
	/* Computers project points using 3 projection matrices and points 3D */

	/* vector X consists of projection matrices and points3D */
	/* each projection matrices has 3x4 coeffs */
	/* each 3D points has X,Y,Z,W(?) */

	/* result of function is projection of N 3D points using 3 projection matrices */
	/* projected points store as (projection by matrix P1),(projection by matrix P2),(projection by matrix P3) */
	/* each projection is x1,y1,x2,y2,x3,y3,x4,y4 */

	/* Compute projection of points */

	/* Fill projection matrices */

	CV_FUNCNAME( "icvFunc_ProjTrifocal" );
	__BEGIN__;

	/* Test data for errors */
	if ( vectX == 0 || resFunc == 0 ) {
		CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
	}

	if ( !CV_IS_MAT(vectX) || !CV_IS_MAT(resFunc) ) {
		CV_ERROR( CV_StsUnsupportedFormat, "Input parameters must be a matrices" );
	}

	int numPoints;
	numPoints = (vectX->rows - 36) / 4;

	if ( numPoints < 1 ) { //!!! Need to correct this minimal number of points
		CV_ERROR( CV_StsUnmatchedSizes, "number of points must be more than 0" );
	}

	if ( resFunc->rows == 2 * numPoints * 3 || resFunc->cols != 1 ) {
		CV_ERROR( CV_StsUnmatchedSizes, "Size of resFunc is not correct it must be 2*numPoints*3 x 1");
	}


	CvMat projMatrs[3];
	double projMatrs_dat[36];
	projMatrs[0] = cvMat(3, 4, CV_64F, projMatrs_dat);
	projMatrs[1] = cvMat(3, 4, CV_64F, projMatrs_dat + 12);
	projMatrs[2] = cvMat(3, 4, CV_64F, projMatrs_dat + 24);

	CvMat point3D;
	double point3D_dat[3];
	point3D = cvMat(3, 1, CV_64F, point3D_dat);

	int currMatr;
	int currV;
	int i, j;

	currV = 0;
	for ( currMatr = 0; currMatr < 3; currMatr++ ) {
		for ( i = 0; i < 3; i++ ) {
			for ( j = 0; j < 4; j++ ) {
				double val = cvmGet(vectX, currV, 0);
				cvmSet(&projMatrs[currMatr], i, j, val);
				currV++;
			}
		}
	}

	/* Project points */
	int currPoint;
	CvMat point4D;
	double point4D_dat[4];
	point4D = cvMat(4, 1, CV_64F, point4D_dat);
	for ( currPoint = 0; currPoint < numPoints; currPoint++ ) {
		/* get curr point */
		point4D_dat[0] = cvmGet(vectX, currV++, 0);
		point4D_dat[1] = cvmGet(vectX, currV++, 0);
		point4D_dat[2] = cvmGet(vectX, currV++, 0);
		point4D_dat[3] = cvmGet(vectX, currV++, 0);

		for ( currMatr = 0; currMatr < 3; currMatr++ ) {
			/* Compute projection for current point */
			cvmMul(&projMatrs[currMatr], &point4D, &point3D);
			double z = point3D_dat[2];
			cvmSet(resFunc, currMatr * numPoints * 2 + currPoint * 2,  0, point3D_dat[0] / z);
			cvmSet(resFunc, currMatr * numPoints * 2 + currPoint * 2 + 1, 0, point3D_dat[1] / z);
		}
	}

	__END__;
	return;
}


/*----------------------------------------------------------------------------------------*/

void icvOptimizeProjectionTrifocal(CvMat** projMatrs, CvMat** projPoints,
								   CvMat** resultProjMatrs, CvMat* resultPoints4D) {

	CvMat* optimX    = 0;
	CvMat* points4D  = 0;
	CvMat* vectorX0  = 0;
	CvMat* observRes = 0;
	//CvMat *error     = 0;

	CV_FUNCNAME( "icvOptimizeProjectionTrifocal" );
	__BEGIN__;

	/* Test data for errors */
	if ( projMatrs == 0 || projPoints == 0 || resultProjMatrs == 0 || resultPoints4D == 0) {
		CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
	}

	if ( !CV_IS_MAT(resultPoints4D) ) {
		CV_ERROR( CV_StsUnsupportedFormat, "resultPoints4D must be a matrix" );
	}

	int numPoints;
	numPoints = resultPoints4D->cols;
	if ( numPoints < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number points of resultPoints4D must be more than 0" );
	}

	if ( resultPoints4D->rows != 4 ) {
		CV_ERROR( CV_StsUnmatchedSizes, "Number of coordinates of points4D must be 4" );
	}

	int i;
	for ( i = 0; i < 3; i++ ) {
		if ( projMatrs[i] == 0 ) {
			CV_ERROR( CV_StsNullPtr, "Some of projMatrs is a NULL pointer" );
		}

		if ( projPoints[i] == 0 ) {
			CV_ERROR( CV_StsNullPtr, "Some of projPoints is a NULL pointer" );
		}

		if ( resultProjMatrs[i] == 0 ) {
			CV_ERROR( CV_StsNullPtr, "Some of resultProjMatrs is a NULL pointer" );
		}

		/* ----------- test for matrix ------------- */
		if ( !CV_IS_MAT(projMatrs[i]) ) {
			CV_ERROR( CV_StsUnsupportedFormat, "Each of projMatrs must be a matrix" );
		}

		if ( !CV_IS_MAT(projPoints[i]) ) {
			CV_ERROR( CV_StsUnsupportedFormat, "Each of projPoints must be a matrix" );
		}

		if ( !CV_IS_MAT(resultProjMatrs[i]) ) {
			CV_ERROR( CV_StsUnsupportedFormat, "Each of resultProjMatrs must be a matrix" );
		}

		/* ------------- Test sizes --------------- */
		if ( projMatrs[i]->rows != 3 || projMatrs[i]->cols != 4 ) {
			CV_ERROR( CV_StsUnmatchedSizes, "Size of projMatr must be 3x4" );
		}

		if ( projPoints[i]->rows != 2 || projPoints[i]->cols != numPoints ) {
			CV_ERROR( CV_StsUnmatchedSizes, "Size of resultProjMatrs must be 3x4" );
		}

		if ( resultProjMatrs[i]->rows != 3 || resultProjMatrs[i]->cols != 4 ) {
			CV_ERROR( CV_StsUnmatchedSizes, "Size of resultProjMatrs must be 3x4" );
		}
	}


	/* Allocate memory for points 4D */
	CV_CALL( points4D  = cvCreateMat(4, numPoints, CV_64F) );
	CV_CALL( vectorX0  = cvCreateMat(36 + numPoints * 4, 1, CV_64F) );
	CV_CALL( observRes = cvCreateMat(2 * numPoints * 3, 1, CV_64F) );
	CV_CALL( optimX    = cvCreateMat(36 + numPoints * 4, 1, CV_64F) );
	//CV_CALL( error     = cvCreateMat(numPoints*2*3,1,CV_64F) );


	/* Reconstruct points 4D using projected points and projection matrices */
	icvReconstructPointsFor3View( projMatrs[0], projMatrs[1], projMatrs[2],
								  projPoints[0], projPoints[1], projPoints[2],
								  points4D);



	/* Fill observed points on images */
	/* result of function is projection of N 3D points using 3 projection matrices */
	/* projected points store as (projection by matrix P1),(projection by matrix P2),(projection by matrix P3) */
	/* each projection is x1,y1,x2,y2,x3,y3,x4,y4 */
	int currMatr;
	for ( currMatr = 0; currMatr < 3; currMatr++ ) {
		for ( i = 0; i < numPoints; i++ ) {
			cvmSet(observRes, currMatr * numPoints * 2 + i * 2  , 0, cvmGet(projPoints[currMatr], 0, i) ); /* x */
			cvmSet(observRes, currMatr * numPoints * 2 + i * 2 + 1, 0, cvmGet(projPoints[currMatr], 1, i) ); /* y */
		}
	}

	/* Fill with projection matrices */
	for ( currMatr = 0; currMatr < 3; currMatr++ ) {
		int i;
		for ( i = 0; i < 12; i++ ) {
			cvmSet(vectorX0, currMatr * 12 + i, 0, cvmGet(projMatrs[currMatr], i / 4, i % 4));
		}
	}

	/* Fill with 4D points */

	int currPoint;
	for ( currPoint = 0; currPoint < numPoints; currPoint++ ) {
		cvmSet(vectorX0, 36 + currPoint * 4 + 0, 0, cvmGet(points4D, 0, currPoint));
		cvmSet(vectorX0, 36 + currPoint * 4 + 1, 0, cvmGet(points4D, 1, currPoint));
		cvmSet(vectorX0, 36 + currPoint * 4 + 2, 0, cvmGet(points4D, 2, currPoint));
		cvmSet(vectorX0, 36 + currPoint * 4 + 3, 0, cvmGet(points4D, 3, currPoint));
	}


	/* Allocate memory for result */
	cvLevenbergMarquardtOptimization( icvJacobianFunction_ProjTrifocal, icvFunc_ProjTrifocal,
									  vectorX0, observRes, optimX, 100, 1e-6);

	/* Copy results */
	for ( currMatr = 0; currMatr < 3; currMatr++ ) {
		/* Copy projection matrices */
		for (int i = 0; i < 12; i++) {
			cvmSet(resultProjMatrs[currMatr], i / 4, i % 4, cvmGet(optimX, currMatr * 12 + i, 0));
		}
	}

	/* Copy 4D points */
	for ( currPoint = 0; currPoint < numPoints; currPoint++ ) {
		cvmSet(resultPoints4D, 0, currPoint, cvmGet(optimX, 36 + currPoint * 4, 0));
		cvmSet(resultPoints4D, 1, currPoint, cvmGet(optimX, 36 + currPoint * 4 + 1, 0));
		cvmSet(resultPoints4D, 2, currPoint, cvmGet(optimX, 36 + currPoint * 4 + 2, 0));
		cvmSet(resultPoints4D, 3, currPoint, cvmGet(optimX, 36 + currPoint * 4 + 3, 0));
	}

	__END__;

	/* Free allocated memory */
	cvReleaseMat(&optimX);
	cvReleaseMat(&points4D);
	cvReleaseMat(&vectorX0);
	cvReleaseMat(&observRes);

	return;


}

/*------------------------------------------------------------------------------*/
/* Create good points using status information */
void icvCreateGoodPoints(CvMat* points, CvMat** goodPoints, CvMat* status) {
	*goodPoints = 0;

	CV_FUNCNAME( "icvCreateGoodPoints" );
	__BEGIN__;

	int numPoints;
	numPoints = points->cols;

	if ( numPoints < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of points must be more than 0" );
	}

	int numCoord;
	numCoord = points->rows;
	if ( numCoord < 1 ) {
		CV_ERROR( CV_StsOutOfRange, "Number of points coordinates must be more than 0" );
	}

	/* Define number of good points */
	int goodNum;
	int i, j;

	goodNum = 0;
	for ( i = 0; i < numPoints; i++) {
		if ( cvmGet(status, 0, i) > 0 ) {
			goodNum++;
		}
	}

	/* Allocate memory for good points */
	CV_CALL( *goodPoints = cvCreateMat(numCoord, goodNum, CV_64F) );

	for ( i = 0; i < numCoord; i++ ) {
		int currPoint = 0;
		for ( j = 0; j < numPoints; j++) {
			if ( cvmGet(status, 0, j) > 0 ) {
				cvmSet(*goodPoints, i, currPoint, cvmGet(points, i, j));
				currPoint++;
			}
		}
	}
	__END__;
	return;
}

